#include "ecat.h"

#include <string.h>
#include <set>
#include <thread>

#include "config.h"
#include "soem/ethercat.h"
#include "jkutil/configparser.h"
#include "log/zuclog.h"

using rtbus::ECat;
using rtbus::ECatImpl;

#define ECAT_ERR(fmt, ...) zuclog_error("ECAT", "%s:" fmt "\n", ifname.c_str(), ##__VA_ARGS__)
#define ECAT_INFO(fmt, ...) zuclog_info("ECAT", "%s:" fmt "\n", ifname.c_str(), ##__VA_ARGS__)

class rtbus::ECatImpl
{
public:
    static const size_t MAX_SLAVES = 64;
    static const size_t MAX_GROUPS = 2;
    struct SdoReq
    {
        uint8_t slave_id;
        uint16_t index;
        uint8_t subindex;
        bool is_write;
        int32_t size;
        uint8_t data[8];

        std::function<int(uint8_t*, int)> cb;
        int timeout_us;
        int retry;
    };

    ECatImpl()
    {
        memset(&ecx_port, 0, sizeof(ecx_port));
        memset(ec_slave, 0, sizeof(ec_slave));
        ec_slavecount = 0;
        memset(ec_group, 0, sizeof(ec_group));
        memset(ec_esibuf, 0, sizeof(ec_esibuf));
        memset(ec_esimap, 0, sizeof(ec_esimap));
        memset(&ec_elist, 0, sizeof(ec_elist));
        memset(&ec_idxstack, 0, sizeof(ec_idxstack));
        EcatError = 0;
        ec_DCtime = 0;
        memset(ec_SMcommtype, 0, sizeof(ec_SMcommtype));
        memset(ec_PDOassign, 0, sizeof(ec_PDOassign));
        memset(ec_PDOdesc, 0, sizeof(ec_PDOdesc));
        memset(&ec_SM, 0, sizeof(ec_SM));
        memset(&ec_FMMU, 0, sizeof(ec_FMMU));
        memset(&cfg, 0, sizeof(cfg));

        ctx = {
            .port = &ecx_port,
            .slavelist = ec_slave,
            .slavecount = &ec_slavecount,
            .maxslave = MAX_SLAVES,
            .grouplist = ec_group,
            .maxgroup = MAX_GROUPS,
            .esibuf = ec_esibuf,
            .esimap = ec_esimap,
            .esislave = 0,
            .elist = &ec_elist,
            .idxstack = &ec_idxstack,
            .ecaterror = &EcatError,
            .DCtime = &ec_DCtime,
            .SMcommtype = ec_SMcommtype,
            .PDOassign = ec_PDOassign,
            .PDOdesc = ec_PDOdesc,
            .eepSM = &ec_SM,
            .eepFMMU = &ec_FMMU,
            .FOEhook = NULL,
            .EOEhook = NULL,
            .manualstatechange = 0,
            .userdata = NULL,
            .enable_systime = &enable_master_systime,
            .systime = &ec_systime,
        };
        enable_master_systime = 0;
        ec_systime = 0;
        integral = 0;
        co_task = nullptr;
        bus_id = 0;
    }

    ~ECatImpl() { stop_co_task(); }
    void stop_co_task()
    {
        do_run = 0;
        if (co_task)
        {
            co_task->join();
            delete co_task;
            co_task = nullptr;
        }
    }

    ecx_contextt ctx;
    ecx_portt ecx_port;
    ec_slavet ec_slave[MAX_SLAVES];
    int ec_slavecount;
    ec_groupt ec_group[MAX_GROUPS];
    uint8 ec_esibuf[EC_MAXEEPBUF];
    uint32 ec_esimap[EC_MAXEEPBITMAP];
    ec_eringt ec_elist;
    ec_idxstackT ec_idxstack;
    boolean EcatError;
    int64 ec_DCtime;
    int32 ec_systime;               //主站系统时间
    boolean enable_master_systime;  //是否启用主站系统时时间,默认关闭则使用的是DC站点的时间，开启则使用ec_systime作为总线时间。用于两个主站之间的时间同步.
    ec_SMcommtypet ec_SMcommtype[EC_MAX_MAPT];
    ec_PDOassignt ec_PDOassign[EC_MAX_MAPT];
    ec_PDOdesct ec_PDOdesc[EC_MAX_MAPT];
    ec_eepromSMt ec_SM;
    ec_eepromFMMUt ec_FMMU;
    std::string ifname;
    int bus_id;
    int64 integral = 0;
    ECat::Config cfg;
    int64_t toff = 0;
    int do_run = 0;
    std::thread* co_task = nullptr;
    std::list<std::shared_ptr<SdoReq>> sdo_list_;
    std::mutex sdo_mtx_;
    void ec_sync(int64 reftime, int64 cycletime, int64* sys_time_offset)
    {
        int64 delta = (reftime - 0) % cycletime;
        if (delta > (cycletime / 2))
        {
            delta = delta - cycletime;
        }
        if (delta > 0)
        {
            integral++;
        }
        if (delta < 0)
        {
            integral--;
        }
        if (sys_time_offset)
        {
            *sys_time_offset = -(delta / 100) - (integral / 20);  //类似PI调节
        }
    }
    int active(int8_t* iomap)
    {
        co_task = new std::thread(&ECatImpl::ecat_co_task, this, iomap);  //启动后台监控任务
        std::this_thread::sleep_for(std::chrono::seconds(2));
        return 0;
    }

    int ecat_co_task(int8_t* iomap)
    {
        //配置线程属性
        std::string task_name = "ecatco" + std::to_string(bus_id);
        pthread_setname_np(pthread_self(), task_name.c_str());
        sched_param sched;
        sched.sched_priority = ECATCO_TASK_PRIO - bus_id;
        pthread_setschedparam(pthread_self(), SCHED_RR, &sched);
        cpu_set_t cpuset;
        CPU_ZERO(&cpuset);
        CPU_SET(ECATCO_TASK_CPU, &cpuset);
        pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset);

        //检查并等待从站数量
        int find_slave_cnt = 0;
        for (uint32_t i = 0; i < cfg.check_slave_timeout; i++)
        {
            std::this_thread::sleep_for(std::chrono::seconds(1));
            find_slave_cnt = ecx_check_slaves_cnt(&ctx);
            if (find_slave_cnt >= (int)cfg.slave_config_.size())
            {
                break;
            }
        }
        if (find_slave_cnt <= 0)
        {
            ECAT_ERR("Don't find slaves.");
            return -1;
        }
        ECAT_INFO("Find slaves: %d, expect: %d", find_slave_cnt, cfg.slave_config_.size());
        /*
        对于JAKA的部分设备，从站上电后会多次复位，这会影响到状态机的切换，
        所以搜索到后需要等待一段时间才能配置
        */
        std::this_thread::sleep_for(std::chrono::seconds(8));

        // 初始化，从init切换到状态preop
        int slaves_wkc = ecx_config_init(&ctx, false);
        if ((int)cfg.slave_config_.size() < slaves_wkc)  // 以实际找到和配置数量中最小个数为准
        {
            slaves_wkc = cfg.slave_config_.size();
        }

        //检查所有从站是否匹配
        bool match_check = true;
        for (int i = 1; i <= slaves_wkc; i++)
        {
            if (ec_slave[i].eep_man != cfg.slave_config_[i - 1].vendor_id || ec_slave[i].eep_id != cfg.slave_config_[i - 1].product_code)
            {
                ECAT_ERR("Slave%d not match, expect: %x-%x, found: %x-%x",
                         i - 1,
                         cfg.slave_config_[i - 1].vendor_id,
                         cfg.slave_config_[i - 1].product_code,
                         ec_slave[i].eep_man,
                         ec_slave[i].eep_id);
                match_check = false;
            }
        }
        if (!match_check)
        {
            return -2;
        }

        //配置从站信息,必须在safeOP前配置
        auto dcret = ecx_configdc(&ctx);
        ECAT_INFO("Has Dc: %d", dcret);
        int mapsize = ecx_config_map_group(&ctx, iomap, 0);
        ECAT_INFO("Map size: %d", mapsize);

        //检查从站PDO配置是否匹配
        for (int i = 1; i < slaves_wkc; i++)
        {
            auto& sc = cfg.slave_config_[i - 1];
            auto& slave_info = ec_slave[i];
            //计算从站配置的总PDO大小
            uint32_t cfg_input_size = 0, cfg_output_size = 0;
            for (auto& entry : sc.txpdos.entries) { cfg_output_size += entry.bitsize; }
            for (auto& entry : sc.rxpdos.entries) { cfg_input_size += entry.bitsize; }
            //检验是否匹配协议栈读取到的PDO大小
            if (ec_slave[i].Ibits != cfg_input_size || ec_slave[i].Obits != cfg_output_size)
            {
                ECAT_ERR(
                    "Slave%d PDO size not match, expect: %d-%d, found: %d-%d", i - 1, cfg_input_size, cfg_output_size, ec_slave[i].Ibits, ec_slave[i].Obits);
                return -2;
            }
        }

        //切换到safeop状态
        ECAT_INFO("Slaves mapped, state to PREOP.");
        ec_slave[0].state = EC_STATE_PRE_OP;
        ecx_writestate(&ctx, 0);
        int ret = ecx_statecheck(&ctx, 0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4);
        ECAT_INFO("ec_statecheck PREOP: %d", ret);

        //配置同步,会计算出DC的启动时间
        for (int i = 1; i <= slaves_wkc; i++)
        {
            ecx_dcsync0(&ctx, i, cfg.active_dc, cfg.slave_config_[i - 1].cycletime_us * 1000, cfg.slave_config_[i - 1].shifttime_us * 1000);
            ECAT_INFO("Slave%d DC:%d synccfg: %d-%d", i - 1, cfg.active_dc, cfg.slave_config_[i - 1].cycletime_us, cfg.slave_config_[i - 1].shifttime_us);
        }

        //切换到safeop状态
        ECAT_INFO("Slaves mapped, state to SAFE_OP.");
        ec_slave[0].state = EC_STATE_SAFE_OP;
        ecx_writestate(&ctx, 0);
        ret = ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4);
        ECAT_INFO("ec_statecheck SAFEOP: %d", ret);

        //切换到OP状态
        usleep(1000 * 1000);  //延迟1s后发送OP请求，不然对于部分设备可能不会响应
        ec_slave[0].state = EC_STATE_OPERATIONAL;
        //TODO:增加retry限制机制
        int op_wkc120 = ecx_writestate(&ctx, 0);
        while (op_wkc120 != slaves_wkc)
        {
            //重试op请求，以解决偶发不响应
            ECAT_ERR("ec_writestate failed: %d", op_wkc120);
            op_wkc120 = ecx_writestate(&ctx, 0);
            usleep(500 * 1000);
        }
        integral = 0;

        do_run = 1;  //启动周期任务

        // wait op
        int chk = 40;
        do {
            // 检测OP状态
            auto resp_stat = ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, EC_TIMEOUTSTATE);
            if (resp_stat != EC_STATE_OPERATIONAL)
            {
                uint16_t stat_code = 0;
                ecx_FPRD(&ecx_port, 0x1001, 0x134, 2, &stat_code, EC_TIMEOUTRET);
                ECAT_INFO("State: %d %d", ec_slave[0].state, stat_code);
            }
        } while (chk-- && (ec_slave[0].state != EC_STATE_OPERATIONAL));
        ECAT_INFO("ALState is OP");

        // OP后，等待dc偏移是否在合理范围内，否则可能在开始时数据不稳定
        if (check_wait_systime_diff(slaves_wkc, 100) != 0)
        {
            return -1;
        }

        do_run = 2;
        for (uint32_t i = 0; do_run > 0; i++)
        {
            std::this_thread::sleep_for(std::chrono::milliseconds(5 * cfg.taskcycle_us / 1000));  //延时以限制带宽
            handle_sdo_list();
            std::this_thread::sleep_for(std::chrono::milliseconds(5 * cfg.taskcycle_us / 1000));
            check_slave();
        }
        ECAT_INFO("ecat_co_task exit.");
        return 0;
    }

    int check_wait_systime_diff(int slaves_wkc, int diff_max_us)
    {
        while (1)
        {
            if (!do_run)  //表明做了deactive或者close
            {
                return -1;
            }
            uint32_t systime_diff = 100000000;
            bool is_ok = true;
            for (int i = 1; i <= slaves_wkc; i++)
            {
#if 1
                ecx_FPRD(&ecx_port, ec_slave[i].configadr, 0x92c, 4, &systime_diff, EC_TIMEOUTRET);
                int d = systime_diff & 0x7fffffff;
                int f = systime_diff & 0x80000000 == 0 ? 1 : -1;
                ECAT_INFO("slave%d systime_diff:%d", i - 1, d * f);
                if (d > 1000 * diff_max_us)
                {
                    is_ok = false;
                }
#else
                int64_t syst = 0;
                ecx_FPRD(&ecx_port, ec_slave[i].configadr, 0x910, 8, &syst, EC_TIMEOUTRET);
                ECAT_INFO("slave%d systime_diff:%ld", i - 1, syst);
#endif
            }
            if (is_ok)
            {
                break;
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(1000));
        }
        ECAT_INFO("System time sync completed");
        return 0;
    }
    void handle_sdo(SdoReq* req)
    {
        for (int i = 0; i < req->retry; i++)
        {
            int wkc = 0;
            int32_t size = req->size;
            if (req->is_write)
            {
                wkc = ecx_SDOwrite(&ctx, req->slave_id, req->index, req->subindex, 0, size, req->data, req->timeout_us);
            }
            else
            {
                wkc = ecx_SDOread(&ctx, req->slave_id, req->index, req->subindex, 0, &size, &req->data, req->timeout_us);
            }
            if (wkc <= 0)
            {
                ECAT_ERR("Slave%d sdo %s 0x%x:%x failed.Retry %d", req->slave_id, req->is_write ? "write" : "read", req->index, req->subindex, i);
                continue;
            }
            uint8_t data[8];
            data[0] = req->slave_id;
            data[1] = req->index & 0xff;
            data[2] = req->index >> 8;
            data[3] = req->subindex & 0xff;
            memcpy(data + 4, req->data, req->size);
            if (req->cb(data, 3 + size))
            {
                ECAT_ERR("SDO %s 0x%x:%x callback failed.Retry %d", req->is_write ? "write" : "read", req->index, req->subindex, i);
                continue;
            }
            break;
        }
    }
    void handle_sdo_list()
    {
        std::lock_guard<std::mutex> lock(sdo_mtx_);
        if (sdo_list_.size() == 0)
            return;
        auto req = sdo_list_.front();
        sdo_list_.pop_front();
        if (!req.get())
        {
            return;
        }
        handle_sdo(req.get());
    }

    void check_slave() {}
};

ECat::ECat() { impl_ = new ECatImpl(); }
ECat::~ECat() { delete impl_; }

int ECat::open(RtCommuItf::BusCfg cfg)
{
    ifname = cfg.adapter_name;
    impl_->ifname = cfg.adapter_name;
    impl_->bus_id = bus_id_;

    int err = ecx_init(&impl_->ctx, ifname.c_str());
    if (err <= 0)
    {
        ECAT_ERR("ecx_init on %s failed.\n", ifname.c_str());
        return -1;
    }
    ECAT_INFO("Open bus:%s at %s success.", name_.c_str(), ifname.c_str());

    std::string eni_file = std::string(ZUC_DEFAULT_CONFIG_DIR) + "/ecat_eni.xml";
    Util::ConfigParser eni(eni_file.c_str());
    if (!eni.is_valid())
    {
        ECAT_ERR("Load eni file %s failed.\n", eni_file.c_str());
        return -1;
    }
    for (int i = 0;; i++)
    {
        //获取设备描述文件名称
        auto exp = Util::ConfigParser::str_append_index("devices.dev", i);
        int id = -1;
        if (eni.get(exp + ".id", id, true) || id < 0)
        {
            break;
        }
        std::string dev_desc_file;
        if (eni.get(exp + ".path", dev_desc_file, true))
        {
            continue;
        }

        //读取ESI文件
        dev_desc_file = std::string(ZUC_DEFAULT_CONFIG_DIR) + "/ecat/" + dev_desc_file;
        Util::ConfigParser dev_desc(dev_desc_file.c_str());
        if (!dev_desc.is_valid())
        {
            ECAT_ERR("Load dev desc file %s failed.\n", dev_desc_file.c_str());
            continue;
        }
        DeviceDescInfo info;
        memset(&info, 0, sizeof(info));
        if (dev_desc.get("Vendor.Id", info.vendor_id))
        {
            continue;
        }
        if (dev_desc.get("Descriptions.Devices.Device.Type.ProductCode", info.product_code, true))
        {
            continue;
        }
        if (dev_desc.get("Descriptions.Devices.Device.Type.RevisionNo", info.version_number, true))
        {
            continue;
        }
        info.esi_file = dev_desc_file;
        // ECAT_INFO("Find descfile: %s, vendor_id: 0x%x, product_code: 0x%x, version: 0x%x",
        //           dev_desc_file.c_str(),
        //           info.vendor_id,
        //           info.product_code,
        //           info.version_number);
        device_desc_.push_back(info);
    }
    return 0;
}
std::string ECat::find_esi_file(int vendor_id, int product_code)
{
    for (auto& info : device_desc_)
    {
        if (info.vendor_id == vendor_id && info.product_code == product_code)
        {
            return info.esi_file;
        }
    }
    return "";
}
void ECat::close() { ecx_close(&impl_->ctx); }
int ECat::config_slave(SlaveInfo& info)
{
    Config::Slave sc;
    sc.cycletime_us = info.cycletime_us;
    sc.shifttime_us = info.shifttime_us;
    sc.position = info.slaveid;
    sc.vendor_id = info.vendor_id;
    sc.product_code = info.product_code;
    sc.revision_number = info.revision_number;
    sc.txpdos.index = 0;
    sc.rxpdos.index = 0;

    //找到对应的ESI文件
    auto esi = find_esi_file(info.vendor_id, info.product_code);
    if (esi == "")
    {
        ECAT_ERR("Find esi file failed, vendor_id: %d, product_code: %d", info.vendor_id, info.product_code);
        return -1;
    }

    //解析ESI文件，获取PDO配置
    Util::ConfigParser p(esi.c_str());
    //找到有配置Sm的TXPDO，并解析
    int txpdoindex = 0;
    std::string tx_exp = "Descriptions.Devices.Device.RxPdo";  //从站的TX即主站的TX
    for (int i = 0;; i++)
    {
        auto pdo_exp = Util::ConfigParser::str_append_index(tx_exp, i);
        int sm = 0;
        if (p.get(pdo_exp + ".Sm", sm, true))
        {
            continue;
        }
        tx_exp = pdo_exp;
        break;
    }
    p.get(tx_exp + ".Index", txpdoindex);
    sc.txpdos.index = txpdoindex;
    int txoffset = 0;
    for (int i = 0;; i++)
    {
        auto entry_exp = Util::ConfigParser::str_append_index(tx_exp + ".Entry", i);
        int entry_index = 0;
        if (p.get(entry_exp + ".Index", entry_index) || entry_index == 0)
        {
            break;
        }
        Config::Slave::Pdo::Entry entry;
        entry.index = entry_index;
        int subindex = 0;
        p.get(entry_exp + ".SubIndex", subindex);
        entry.subindex = subindex;
        int bitsize = 0;
        p.get(entry_exp + ".BitLen", bitsize);
        entry.bitsize = bitsize;
        entry.offset = txoffset;
        txoffset += bitsize / 8;
        sc.txpdos.entries.push_back(entry);
    }
    //找到有配置Sm的RXPDO，并解析
    int rxpdoindex = 0;
    std::string rx_exp = "Descriptions.Devices.Device.TxPdo";  //从站的TX即主站的RX
    for (int i = 0;; i++)
    {
        auto pdo_exp = Util::ConfigParser::str_append_index(rx_exp, i);
        int sm = 0;
        if (p.get(pdo_exp + ".Sm", sm, true))
        {
            continue;
        }
        rx_exp = pdo_exp;
        break;
    }
    p.get(rx_exp + ".Index", rxpdoindex);
    sc.rxpdos.index = rxpdoindex;
    int rxoffset = 0;
    for (int i = 0;; i++)
    {
        auto entry_exp = Util::ConfigParser::str_append_index(rx_exp + ".Entry", i);
        int entry_index = 0;
        if (p.get(entry_exp + ".Index", entry_index) || entry_index == 0)
        {
            break;
        }
        Config::Slave::Pdo::Entry entry;
        entry.index = entry_index;
        int subindex = 0;
        p.get(entry_exp + ".SubIndex", subindex);
        entry.subindex = subindex;
        int bitsize = 0;
        p.get(entry_exp + ".BitLen", bitsize);
        entry.bitsize = bitsize;
        entry.offset = rxoffset;
        rxoffset += bitsize / 8;
        sc.rxpdos.entries.push_back(entry);
    }

    cfg_.slave_config_.push_back(sc);
    return RtCommuItf::config_slave(info);
}
void ECat::sync_time(RtCommuItf* master)
{
    if (master == nullptr || master == this || !cfg_.active_dc)
    {
        impl_->enable_master_systime = false;
        return;
    }

    impl_->enable_master_systime = true;
    static int first = 1;
    static int64_t master_off = 0;
    if (first)
    {
        auto ct1 = dynamic_cast<ECat*>(master)->impl_->ec_DCtime;
        auto ct2 = impl_->ec_DCtime;
        if (ct1 && ct2)
        {
            ECAT_INFO("Master systime: %ld %ld", ct1, ct2);
            master_off = ct2 - ct1;
            first = 0;
        }
    }
    if (!first)
    {
        impl_->ec_systime = dynamic_cast<ECat*>(master)->impl_->ec_DCtime + master_off + 2 * cfg_.taskcycle_us * 1000;
    }
}
int ECat::load_cfg(struct Config& cfg)
{
    //遍历所有pdo配置来构造config
    cfg.adapter = impl_->ifname;
    if (slaves_info_.empty())
    {
        cfg.taskcycle_us = 8000;
        return -1;
    }
    else
    {
        cfg.taskcycle_us = slaves_info_[0].cycletime_us;
        cfg.active_dc = slaves_info_[0].enable_dc;
    }
    cfg.check_slave_timeout = 10;

    return 0;
}
int ECat::active()
{
    if (actived_flag_ == 1 || actived_flag_ == 2)
    {
        return 0;
    }
    load_cfg(cfg_);
    actived_flag_ = 1;
    impl_->cfg = cfg_;
    return impl_->active(iomap_);
}

int ECat::deactive()
{
    if (actived_flag_ == 0)
        return 0;

    //切换到preop状态
    impl_->ec_slave[0].state = EC_STATE_PRE_OP;
    int wkc = 0;
    int retry = 3;
    do {
        wkc = ecx_writestate(&impl_->ctx, 0);
        usleep(100);
    } while (wkc != 0 && retry--);

    //停止co_task
    impl_->do_run = 0;
    actived_flag_ = 0;
    impl_->stop_co_task();
    return 0;
}
int ECat::register_sdo_handle(uint8_t slave_id, uint32_t write_req_frame_id, uint32_t read_req_frame_id, uint32_t resp_frame_id, size_t obj_id_size)
{
    //ethercat无需配置sdo即可使用
    return 0;
}

void ECat::recv(int64_t* time_off)
{
    if (!impl_->do_run)
    {
        return;
    }
    actived_flag_ = 2;
    auto wkc = ecx_receive_processdata(&impl_->ctx, 200);

    //计算DC漂移补偿time_off
    if (impl_->ec_slave[0].hasdc && cfg_.active_dc)
    {
        impl_->ec_sync(impl_->ec_DCtime, impl_->cfg.taskcycle_us * 1000, time_off);
    }
    if (wkc >= 0)
    {
        for (size_t i = 0; i < (size_t)impl_->ec_slavecount; i++)
        {
            if (i >= impl_->cfg.slave_config_.size())
                break;
            auto& sc = impl_->cfg.slave_config_[i];

            if (impl_->do_run == 2)  //必须同步偏差收敛阈值后才开始发送txpdo，以保证从站DC中断处理的数据的稳定性
            {
                //设置txpdo数据
                for (auto& entry : sc.txpdos.entries)
                {
                    int slave_index = sc.position;
                    int index = entry.index << 8 | entry.subindex;
                    uint32_t objid = slave_index << 24 | index;

                    auto data = impl_->ec_slave[slave_index + 1].outputs + entry.offset;
                    if (txpdo_.count(objid))
                    {
                        auto& obj = txpdo_[objid];
                        memcpy(data, obj.data, obj.size);
                        // ECAT_INFO("Slave%d txpdo: %x-%x-%d: %x %x %x %x", slave_index, entry.index, entry.subindex, entry.bitsize, data[0], data[1], data[2], data[3]);
                    }
                }
            }
            //获取rxpdo数据
            for (auto& entry : sc.rxpdos.entries)
            {
                int slave_index = sc.position;
                int index = entry.index << 8 | entry.subindex;
                uint32_t objid = slave_index << 24 | index;

                auto data = impl_->ec_slave[slave_index + 1].inputs + entry.offset;
                if (rxpdo_.count(objid))
                {
                    auto& obj = rxpdo_[objid];
                    obj.alive_cnt++;
                    memcpy(obj.data, data, obj.size);
                    // ECAT_INFO("Slave%d rxpdo: %x-%x-%d: %x %x %x %x", slave_index, entry.index, entry.subindex, entry.bitsize, data[0], data[1], data[2], data[3]);
                }
            }
        }
    }
    ecx_send_processdata(&impl_->ctx);
}

void ECat::send()
{
    //recv的地方已经处理了数据的赋值发送，这里不需要再处理
}

void ECat::reset()
{
    deactive();
    slaves_info_.clear();
    txpdo_.clear();
    rxpdo_.clear();
}
int ECat::get_sdo(uint8_t slave_id, uint32_t obj_id, std::function<int(uint8_t*, int)> cb, int timeout, int retry)
{
    ECatImpl::SdoReq req;
    req.slave_id = slave_id;
    req.index = obj_id >> 8;
    req.subindex = obj_id & 0xff;
    req.is_write = false;
    req.size = 4;
    req.cb = cb;
    req.timeout_us = timeout * 1000;
    req.retry = retry;

    std::lock_guard<std::mutex> lock(impl_->sdo_mtx_);
    impl_->sdo_list_.push_back(std::make_shared<ECatImpl::SdoReq>(req));

    return 0;
}
int ECat::set_sdo(uint8_t slave_id, uint32_t obj_id, uint8_t val[4], size_t size, std::function<int(uint8_t*, int)> cb, int timeout, int retry)
{
    ECatImpl::SdoReq req;
    req.slave_id = slave_id;
    req.index = obj_id >> 8;
    req.subindex = obj_id & 0xff;
    req.is_write = true;
    req.size = size;
    memcpy(req.data, val, size);
    req.cb = cb;
    req.timeout_us = timeout * 1000;
    req.retry = retry;

    std::lock_guard<std::mutex> lock(impl_->sdo_mtx_);
    impl_->sdo_list_.push_back(std::make_shared<ECatImpl::SdoReq>(req));

    return 0;
}
int ECat::get_sdo_blocking(uint8_t slave_id, uint32_t objid, void* data, int data_len, int retry)
{
    int ret = 0;
    for (int i = 0; i < retry; i++)
    {
        int wkc = 0;
        int32_t size = data_len;

        wkc = ecx_SDOread(&impl_->ctx, slave_id, objid >> 8 & 0xffff, objid & 0xff, 0, &size, &size, 100000);

        if (wkc <= 0)
        {
            ECAT_ERR("Slave%d sdo read 0x%x:%x failed.Retry %d", slave_id, objid >> 8 & 0xffff, objid & 0xff, i);
            ret = -1;
            continue;
        }
        else
        {
            ret = 0;
        }
        break;
    }
    return ret;
}
int ECat::set_sdo_blocking(uint8_t slave_id, uint32_t objid, void* data, int s, void* respdata, int retry)
{
    int ret = 0;
    for (int i = 0; i < retry; i++)
    {
        int wkc = 0;
        int32_t size = s;

        wkc = ecx_SDOwrite(&impl_->ctx, slave_id, objid >> 8 & 0xffff, objid & 0xff, 0, size, data, 100000);

        if (wkc <= 0)
        {
            ECAT_ERR("Slave%d sdo read 0x%x:%x failed.Retry %d", slave_id, objid >> 8 & 0xffff, objid & 0xff, i);
            ret = -1;
            continue;
        }
        else
        {
            ret = 0;
            if (respdata)
            {
                memcpy(respdata, data, s);
            }
        }
        break;
    }
    return ret;
}